#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <arm_navigation_msgs/MoveArmAction.h>
#include <arm_navigation_msgs/GetStateValidity.h>
#include <arm_navigation_msgs/GetRobotState.h>
#include <planning_environment/models/collision_models.h>

bool isValid(const sensor_msgs::JointState& state, ros::ServiceClient& client)
{
  arm_navigation_msgs::GetStateValidity::Request req;
  for(unsigned int i = 0; i < state.name.size(); ++i)
  {
    req.robot_state.joint_state.name.push_back(state.name[i]);
    req.robot_state.joint_state.position.push_back(state.position[i]);
  }

  req.robot_state.joint_state.header.stamp = ros::Time::now();
  req.check_collisions = true;

  arm_navigation_msgs::GetStateValidity::Response res;

  if(client.call(req, res))
  {
    if(res.error_code.val == res.error_code.SUCCESS)
    {
      //ROS_INFO("Requested state is not in collision");
      return true;
    }
    else
    {
      //ROS_INFO("Requested state is in collision. Error code: %d",res.error_code.val);
      return false;
    }
  }
  else
  {
    ROS_ERROR("Service call to check state validity failed %s", client.getService().c_str());
    return true;
  }
}

void getState(sensor_msgs::JointState& state, ros::ServiceClient& client, std::vector<std::string>& names)
{
  arm_navigation_msgs::GetRobotState::Request req;
  arm_navigation_msgs::GetRobotState::Response res;

  if(client.call(req, res))
  {
    for(unsigned int i = 0; i < res.robot_state.joint_state.name.size(); ++i)
    {
      std::string name = res.robot_state.joint_state.name[i];
      if(find(names.begin(), names.end(), name) != names.end())
      {
        state.name.push_back(res.robot_state.joint_state.name[i]);
        state.position.push_back(res.robot_state.joint_state.position[i]);
      }
    }
  }
}

void satisfyJointLimit(std::string name, double& position, planning_environment::CollisionModels* collision_model)
{
  std::pair<double, double> joint_limit;
  collision_model->getKinematicModel()->getJointModel(name)->getVariableBounds(name, joint_limit);
  if(position < joint_limit.first) position = joint_limit.first + 0.001;
  else if(position > joint_limit.second) position = joint_limit.second - 0.001;
}

void sampleValidState(planning_environment::CollisionModels* collision_model, ros::ServiceClient& validChecker, const std::vector<std::string>& names, sensor_msgs::JointState& state)
{
  std::vector<std::pair<double, double> > joint_limits;
  for(unsigned int i = 0; i < names.size(); ++i)
  {
    std::string name = names[i];
    std::pair<double, double> joint_limit;
    collision_model->getKinematicModel()->getJointModel(name)->getVariableBounds(name, joint_limit);
    joint_limits.push_back(joint_limit);
    state.name.push_back(name);
  }
  state.position.resize(names.size());

  while(1)
  {
    for(unsigned int i = 0; i < names.size(); ++i)
    {
      double rand_position = (joint_limits[i].second - joint_limits[i].first) * rand() / (double)RAND_MAX + joint_limits[i].first;
      state.position[i] = rand_position;
    }

    if(isValid(state, validChecker))
        break;
  }
}


int main(int argc, char **argv)
{
  ros::init (argc, argv, "move_arm_joint_goal_test");
  ros::NodeHandle nh;
  actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm", true);

  planning_environment::CollisionModels* collision_models_ = new planning_environment::CollisionModels("robot_description");

  ros::service::waitForService("planning_scene_validity_server/get_state_validity");
  ros::ServiceClient check_state_validity_client_ = nh.serviceClient<arm_navigation_msgs::GetStateValidity>("planning_scene_validity_server/get_state_validity");
  ros::service::waitForService("environment_server/get_robot_state");
  ros::ServiceClient get_robot_state_client_ = nh.serviceClient<arm_navigation_msgs::GetRobotState>("environment_server/get_robot_state");

  move_arm.waitForServer();
  ROS_INFO("Connected to server");

  arm_navigation_msgs::MoveArmGoal goalB;
  std::vector<std::string> names(7);
  names[0] = "r_shoulder_pan_joint";
  names[1] = "r_shoulder_lift_joint";
  names[2] = "r_upper_arm_roll_joint";
  names[3] = "r_elbow_flex_joint";
  names[4] = "r_forearm_roll_joint";
  names[5] = "r_wrist_flex_joint";
  names[6] = "r_wrist_roll_joint";

  goalB.motion_plan_request.group_name = "right_arm";
  goalB.motion_plan_request.num_planning_attempts = 1;
  goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);

  goalB.motion_plan_request.planner_id= std::string("");
  goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
  goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());

  sensor_msgs::JointState curState;
  getState(curState, get_robot_state_client_, names);




  {

    for(unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
    {
      goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
      goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
      goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
      goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
    }

    goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
    goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
    goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.15;


    bool finished_within_time = false;
    move_arm.sendGoal(goalB);
    finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
    if (!finished_within_time)
    {
      move_arm.cancelGoal();
      ROS_INFO("Timed out achieving goal A");
    }
    else
    {
      actionlib::SimpleClientGoalState state = move_arm.getState();
      bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
      if(success)
        ROS_INFO("Action finished: %s",state.toString().c_str());
      else
        ROS_INFO("Action failed: %s",state.toString().c_str());
    }
  }


  if (nh.ok())
  {
    for(int n = 0; n < 10; ++n)
    {
      sensor_msgs::JointState goalState;
      sampleValidState(collision_models_, check_state_validity_client_, names, goalState);

      for(unsigned int i = 0; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
      {
        goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
        goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = goalState.position[i];
        goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
        goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
      }


      bool finished_within_time = false;
      move_arm.sendGoal(goalB);
      finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
      if (!finished_within_time)
      {
        move_arm.cancelGoal();
        ROS_INFO("Timed out achieving goal A");
      }
      else
      {
        actionlib::SimpleClientGoalState state = move_arm.getState();
        bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
        if(success)
          ROS_INFO("Action finished: %s",state.toString().c_str());
        else
          ROS_INFO("Action failed: %s",state.toString().c_str());
      }
    }
  }

  delete collision_models_;

  ros::shutdown();
}
